#include "Includes.h"
///////////////////////////////////////////////////////////////////////////////////////
void wait_task(short taskID)
{
	while(1)
	{
		if( getTaskState(taskID)==taskStateStopped)
			break;
		wait1Msec(50);
	}
}
///
void wait_two_task( short taskID_1,short taskID_2)
{
	while(1)
	{
		if( getTaskState(taskID_1)==taskStateStopped && getTaskState(taskID_2)==taskStateStopped)
			break;
		wait1Msec(50);
	}
}
///
void wait_sem_rise_updown()
{
	while(1)
	{
		SemaphoreLock(sem_Rise_Updown,100);
		if(bDoesTaskOwnSemaphore(sem_Rise_Updown) )
		{
			SemaphoreUnlock(sem_Rise_Updown);
			writeDebugStream("sem rise: %d\n",time1[GBL_Rise_Timer_x]);
			return;
		}
	}
}
///
void wait_sem_turn_arm()
{
	while(1)
	{
		SemaphoreLock(sem_Turning_Arm,100);
		if(bDoesTaskOwnSemaphore(sem_Turning_Arm) )
		{
			SemaphoreUnlock(sem_Turning_Arm);
			writeDebugStream("sem turn: %d\n",time1[GBL_Turning_Arm_Timer_x]);
			return;
		}
	}
}
///
void wait_sem_rise_updown_and_turn_arm()
{
	wait_sem_rise_updown();
	wait_sem_turn_arm();
}
///
task Shake_Arm()
{
	int k=0;
	int dir=1;
	for(k=0;k<10;k++)
	{
		Turning_Arm_Func(dir*40);
		dir=-dir;
		wait1Msec(80);
	}
	Turning_Arm_Func(0);
}
/////////////////////////////////////////
task Down_Lowest_Get_Section_task()
{
	Rise_UpDown_Func(-125);
	while( !Touch_Limit_Down_Height )
	{
		wait1Msec(15);
	}
	Rise_UpDown_Func(-50);
}
/////
void wait_insert()
{
	wait_sem_rise_updown();
}
////
void wait_lowest()
{
	while(!Touch_Limit_Down_Height)
		wait1Msec(10);
}
/////
#define delta_src_shoulder 100
task wait_to_start_down()
{
	if(FIELD==Red_Field)
	{
		while(1)
		{
			if(nMotorEncoder[Mtr_Arm]<=src_deg_Red-delta_src_shoulder)
				StopTask(wait_to_start_down);
			wait1Msec(5);
		}
	}
	if(FIELD==Blue_Field)
	{
		while(1)
		{
			if(nMotorEncoder[Mtr_Arm]>=src_deg_Blue-delta_src_shoulder)
				StopTask(wait_to_start_down);
			wait1Msec(5);
		}
	}
}
/////
#define hgt_start_turn_arm 435
task wait_to_start_turn_arm()
{
	while(1)
	{
		if(SensorValue[Angle_Height]>GBL_hgt_Lowest+hgt_start_turn_arm)
			StopTask(wait_to_start_turn_arm);
		wait1Msec(5);
	}
}
/////
